#include <iostream>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Dense>

using namespace Eigen;
using namespace std;

double Rad_to_deg = 180.0 / M_PI;
double Deg_to_rad = M_PI / 180.0;
/**
 * @brief 转换矩阵DCM
 * Rbe表示机体坐标系到大地坐标系（导航坐标系）的转换矩阵
 *
 */
void test01()
{
    Matrix3d Rbe;
    float yaw = 60.0 * Deg_to_rad;
    float pitch = 0.0f;
    float roll = 0.0f;
    Rbe = AngleAxisd(yaw, Vector3d::UnitZ());
    // Rbe = AngleAxisd(yaw, Vector3d::UnitZ()) * AngleAxisd(pitch, Vector3d::UnitY()) * AngleAxisd(roll, Vector3d::UnitX());
    std::cout << Rbe << std::endl;

    //
    Matrix3d Reb = Rbe.transpose();
    std::cout << Reb << std::endl;
}

void test02()
{
    Matrix3d Rbe;
    float yaw = 60.0 * Deg_to_rad;
    Rbe = AngleAxisd(yaw, Vector3d::UnitZ());
    std::cout << Rbe << std::endl;

    // 对一个向量进行转换
    Vector3d vb(-0.5f, -1.0f, 0.0f);
    Vector3d ve = Rbe * vb;
    std::cout << ve << std::endl;
    // 对多个向量进行转换
    Eigen::Matrix3d shape_triangle{};
    shape_triangle << 0.0, -0.5, 0.5,
        0.0, -1.0, -1.0,
        0.0, 0.0, 0.0; // 按行输入
    std::cout << shape_triangle << std::endl;
    std::cout << Rbe * shape_triangle << std::endl;
}

void test03()
{
    Matrix3d Rbe;
    // float yaw = 352 * 3.14f / 180;
    float yaw = 3.0f;
    Rbe = AngleAxisd(yaw, Vector3d::UnitZ());
    std::cout << Rbe << std::endl;

    // 对一个向量进行转换
    // Vector3d vb(-0.5f, -1.0f, 0.0f);
    Vector3d vb(4.7f, 1.4f, 0.0f);
    Vector3d ve = Rbe * vb;
    std::cout << ve << std::endl;
}

int main(int argc, char *argv[])
{
    // test01();
    // test02();
    test03();
    return 0;
}
